#!/usr/bin/env python3
import rospy
from std_msgs.msg import Float64MultiArray, String, Int32

def main():
    rospy.init_node('param_setter_node')

    pub = rospy.Publisher('/command_move_joint', Float64MultiArray, queue_size=10)

    rospy.sleep(1)

    msg = Float64MultiArray()
    msg.data = [0.0, -0.78, 0.0, -1.5, 0.0, 1.0, 1.5708]
    pub.publish(msg)
    rospy.sleep(5)

    # 设置向量（列表）参数
    rospy.set_param('/robot_manager_node/mit_kp', [500.0, 500.0, 200.0, 200.0, 100.0, 100.0, 100.0])
    rospy.set_param('/robot_manager_node/mit_kd', [10.0, 10.0, 10.0, 10.0, 1.0, 1.0, 1.0])

    rospy.loginfo("Parameters have been set on the parameter server.")

    msg = Float64MultiArray()
    msg.data = [0.0, -0.78, 0.0, -1.5, 0.0, 1.0, 1.5708]
    pub.publish(msg)

if __name__ == '__main__':
    main()
